#include "Chassis.h"
#include "gpio.h"
#include "Action.h"
#include "abs.h"


static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
	if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */
     if (ctrl->C_cmd.type== c->ctrl && ctrl->C_cmd.mode== c->mode) return CHASSIS_OK; /*模式未改变直接返回*/

  //此处源代码处做了pid的reset 待添加

   c->ctrl =ctrl->C_cmd.type;
	 c->mode =ctrl->C_cmd.mode;
   
   return CHASSIS_OK;
}

//该函数用来更新can任务获得的电机反馈值
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
  if (c == NULL) return CHASSIS_ERR_NULL;
  if (can == NULL) return CHASSIS_ERR_NULL;
  for (uint8_t i = 0; i < 4; i++) {
    c->motorfeedback.rotor_angle6020[i] = can->motor.chassis6020.as_array[i].rotor_angle;
    c->motorfeedback.rotor_rpm6020[i] = 
        can->motor.chassis6020.as_array[i].rotor_speed;

    c->motorfeedback.rotor_angle5065[i] = can->motor.chassis5065.as_array[i].rotor_angle;
	  c->motorfeedback.rotor_rpm5065[i] = can->motor.chassis5065.as_array[i].rotor_speed;
	  c->motorfeedback.torque_current[i] = can->motor.chassis5065.as_array[i].torque_current;
  }

  return CHASSIS_OK;
}




int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param)
{
    if (c == NULL) return CHASSIS_ERR_NULL;

  c->param = param;             /*初始化参数 */
   for(int i=0 ; i < 4; i ++)
	{
   c->wheelPolar[i] = cartesianToPolar(c->param->AgvZeroPos.WheelXpos[i],c->param->AgvZeroPos.WheelYpos[i]);
    }
	
		//底盘pid初始化
	     for(uint8_t i = 0 ; i<4 ; i++)
		 {
		     PID_init(&(c->pid.chassis_6020OmegaPid[i]), PID_POSITION,&(c->param->C6020Omega_param));
			 PID_init(&(c->pid.chassis_6020anglePid[i]), PID_POSITION,&(c->param->C6020Angle_param) ); 
	   }
	 /*角度纠正PID */
	      PID_init(&(c->pid.chassis_AngleCorPID),PID_POSITION,&(c->param->AngleCor_param));//外环
	    PID_init(&(c->pid.chassis_OmegaCorPID),PID_POSITION,&(c->param->OmegaCor_param));//内环

	 return CHASSIS_OK;
}



//对单个舵轮进行角度解算的函数
PolarCoordinate_t wheelWCompute(PolarCoordinate_t wheel, float w){
    PolarCoordinate_t wheelout;
    wheelout.theta = wheel.theta + 0.5f * M_PI;
    wheelout.r = wheel.r * w;
    return wheelout;
}

//选择转动最短的弧

void arc_rotation_selct(Chassis_t *c,PolarCoordinate_t *wheelStatus)
{
  fp32 angle_get[4];//获取量纲转化后的角度 弧度制下的6020反馈角度
  fp32 zero_point_pi[4];//获取弧度制下的6020零点角度
	fp32 rela_angle[4];//相对角度
	for(uint8_t i =0 ; i<4 ; i++)
	{
	 angle_get[i] = AngleChange(RADIAN,c->motorfeedback.rotor_angle6020[i]);
	 zero_point_pi[i] =AngleChange(RADIAN,c->param->Agv6020zeroPos[i]);
		rela_angle[i] =map_fp32(AngleCalc(RELATIVE,angle_get[i],zero_point_pi[i]),0,M_2PI,M_2PI,0);//由于6020安装位置导致的电机正反转不同

	if(_normalizeAngle((wheelStatus[i].theta)-rela_angle[i]) > M_PI/2 && _normalizeAngle((wheelStatus[i].theta) - rela_angle[i]) < (M_PI*3)/2)
	{
		c->hopemotorout.AgvAngleOut[i] =_normalizeAngle(wheelStatus[0].theta + M_PI);
		c->hopemotorout.AgvAngleOut[i] =AngleChange(DEGREE,c->hopemotorout.AgvAngleOut[i]);//(0-360)
		c->hopemotorout.AgvSpeedOut[i] =-wheelStatus[i].r;
	}
	else 
	{
		c->hopemotorout.AgvAngleOut[i] =AngleChange(DEGREE,_normalizeAngle(wheelStatus[i].theta));//转化成ֵ(0-360)
		c->hopemotorout.AgvSpeedOut[i] =wheelStatus[i].r;
	}
	}

	
}

void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw,PolarCoordinate_t *wheelStatus)  //底盘逆运动学的解算
{		
		//底盘解算函数
    PolarCoordinate_t wheelStatuVxoy;
    PolarCoordinate_t wheelStatuVw[4];
    wheelStatuVxoy = cartesianToPolar(Vx, Vy);
    for(uint8_t i=0; i <4; i++)
		{
    wheelStatuVw[i] = wheelWCompute(c->wheelPolar[i], Vw);
    wheelStatus[i] = addPolarVectors(wheelStatuVxoy, wheelStatuVw[i]);         
		} 
   	if ((wheelStatus[0].r == 0) && (wheelStatus[1].r == 0) &&  (wheelStatus[2].r == 0) &&  (wheelStatus[3].r == 0))
		{
			wheelStatus[0].theta = 0.5f*M_PI;
			wheelStatus[1].theta = 0.5f*M_PI;
			wheelStatus[2].theta = 0.5f*M_PI;
			wheelStatus[3].theta = 0.5f*M_PI;
		}
		arc_rotation_selct(c,wheelStatus);//优化转动路径
		
			if(c->ctrl ==OPS)
	{
		//此处为速度量纲的转化 转化为真实的速度值(对接码盘的需要)
		for(int i =0; i <4 ; i++)
		{
			c->hopemotorout.AgvSpeedOut[i] =  c->hopemotorout.AgvSpeedOut[i]/M3508_MOTOR_RPM_TO_VECTOR;
		}
	}
	
	
		for(int j =0; j < 4; j++)
		{	
		c->final_out.final_5065out[j]=c->hopemotorout.AgvSpeedOut[j];
		}
  
		
}



fp32 imu_posctrl(Chassis_t *c,CMD_t *ctrl)
{
 if(ctrl->C_cmd.type ==RC &&ctrl->C_cmd.mode ==IMU)
 {
	 static uint8_t flag=0;
	 static fp32 hopeangle =0;
	 fp32 dta;
	 if(ctrl->Vw !=0) flag=1;
	 else if(ctrl->Vw ==0 &&flag ==1)
	 {
		 flag =0;
		 hopeangle = c->pos088.imu_eulr.yaw;
	 }
	 dta = -PID_calc(&(c->pid.chassis_AngleCorPID),c->pos088.imu_eulr.yaw,hopeangle);
	 
	  return dta;
 }
 else return 0;

}

int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,PolarCoordinate_t *wheelStatus,CAN_Output_t *out)
{
	if(c ==NULL)  return CHASSIS_ERR_NULL;
	if(ctrl ==NULL)  return CHASSIS_ERR_NULL;
	
	/*  AGV相对角度是指相对于底盘中零点的角度 绝对角度指的是电机的角度*/
		fp32 angle_difference[4];//获取期望角度和原角度经过最短路径后的差值
		fp32 abs_angle[4];//经过转化的绝对角度
		fp32 normal_angle[4];//获取经过映射后的6020反馈角度
		fp32 delta_angle[4];//计算出的期望角度差值

	Chassis_SetCtrl(c,ctrl);
	
	switch (c->ctrl)
	{
	case RC:
	//在cmd里对数据进行处理 包括方向和油门
      c->move_vec.Vw = ctrl->Vw;
		  c->move_vec.Vx = ctrl->Vx;
		  c->move_vec.Vy = ctrl->Vy;
	    break;
	case CAMERA_PICK:
		  if(ctrl->C_pick.cmd == 'Y')
			{
				
			}
			else if(ctrl->C_pick.cmd == 'N')
			{
			
			}
			else
			return DEVICE_ERR;
		  break;
	case CAMERA_PUT:
		  break;
	case SLAM_NAVI:
		  c->move_vec.Vw =ctrl->C_navi.wz;
	    c->move_vec.Vx =ctrl->C_navi.vx;
	    c->move_vec.Vy =ctrl->C_navi.vy;
		  break;
	
	case OPS:
	//		go_path();
	     break;
	}
	switch (c->mode)
	{
	case RELAXED:
				c->move_vec.Vx =0;
				c->move_vec.Vy =0;
				c->move_vec.Vw =0;
		break;
	//在需要由陀螺仪来固定一个方向的时候使用 在PID中修改期望的角度值
	case IMU:
	      c->move_vec.Vw = imu_posctrl(c,ctrl)+c->move_vec.Vw;
	  break;
	
	}
  //逆运动学解算出期望的电机值
  Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw,wheelStatus);
	uint8_t en_vesc=0;
	//速度赋值
	for(uint8_t i = 0 ; i < 4 ; i++)
	{
		normal_angle[i] =map_fp32(c->motorfeedback.rotor_angle6020[i],0,360.0f,360.0f,0);
		abs_angle[i] =c->hopemotorout.AgvAngleOut[i];
		abs_angle[i]=_normalizeTo360(abs_angle[i] - c->param->Agv6020zeroPos[i]);
		angle_difference[i] = calculate_rotation(normal_angle[i], abs_angle[i]);
		if (abs_float_single(angle_difference[i])>30) en_vesc = 1;              //判断角度误差
    delta_angle[i] = PID_calc(&(c->pid.chassis_6020anglePid[i]),angle_difference[i],0);
		c->final_out.final_6020out[i] = PID_calc(&(c->pid.chassis_6020OmegaPid[i]),c->motorfeedback.rotor_rpm6020[i],delta_angle[i]);

		//将最终的电机期望的输出值通过消息队列发送给can任务
		out->chassis6020.as_array[i] = c->final_out.final_6020out[i];
		
		if (en_vesc==0)  out->chassis5065.erpm[i] = (c->final_out.final_5065out[i])*7;   //如果角度误差过大就不更新vesc命令
	}
	return CHASSIS_OK;
}



